#include "stdlib.h"
#include "math.h"
#include "float.h"                           // required for DBL_EPSILON
#include "string.h"                                  // required for memcpy()

#include "core.h"
#include "vector3D.h"
#include "coordinate3D.h"
#include "robot.h"
#include "joints.h"
#include "animation.h"
#include "nrutil.h"

#include <QDebug>
#include <QFileDialog>
#include <QFile>
#include <QMessageBox>


Core::Core(QObject *mainwin, Robot *Robot)
{
	parent = mainwin;
	robot = Robot;
	
	roundPrecision = 10000;
}

Core::~Core()
{
	//clean up
}

void Core::init()
{


}

void Core::setRobotPointer(Robot *Robot)
{
	robot = robot;
}

//for building mode
void Core::loadBlankRobot()
{

}

void Core::loadScaraRobot()
{
	robot->reset();
	robot->addJoint(Joint(REVOLUTE));
	robot->addJoint(Joint(REVOLUTE));
	robot->addJoint(Joint(PRISMATIC));
	robot->addJoint(Joint(GRIPPER));
	
	vector3D up(0.0, 1.0, 0.0);
	vector3D down(0.0, -1.0, 0.0);
	
	robot->jointList[0].setCenter(coord3D(0.0, 0.0, 0.0));
        robot->jointList[0].setStaticCenter(coord3D(0.0, 0.0, 0.0));
	robot->jointList[0].setRotationLimits(-175.0, 175.0);
	robot->jointList[0].setOrientationK(up);
        robot->jointList[0].setStaticOrientationK(up);
	robot->jointList[0].setTranslationK(0.0);
	
	robot->jointList[1].setCenter(coord3D(1.5, 1.0, 0.0));
        robot->jointList[1].setStaticCenter(coord3D(1.5, 0.0, 0.0));
	robot->jointList[1].setRotationLimits(-175.0, 175.0);
	robot->jointList[1].setOrientationK(up);
        robot->jointList[1].setStaticOrientationK(up);
	robot->jointList[1].setTranslationK(1.5);

	robot->jointList[2].setCenter(coord3D(3.0, 2.0, 0.0));
        robot->jointList[2].setStaticCenter(coord3D(3.0, 2.0, 0.0));
	robot->jointList[2].setExtensionLimits(0.0, 1.0);
	robot->jointList[2].setOrientationK(down);
        robot->jointList[2].setStaticOrientationK(down);
	robot->jointList[2].setTranslationK(1.5);

	robot->jointList[3].setCenter(coord3D(3.0, -0.8, 0.0));
        robot->jointList[3].setStaticCenter(coord3D(3.0, -0.8, 0.0));
	robot->jointList[3].setOrientationK(down);
        robot->jointList[3].setStaticOrientationK(down);
	robot->jointList[3].setTranslationK(0.0);

}


void Core::adjustJoint(int joint, float value)	//SLOT
{
	robot->jointList[joint].actuate(value);

	recalculateJoints();
}

float Core::round(float number)
{
	float returnValue;
	
	returnValue = float(floor(number * roundPrecision))/float(roundPrecision);
	
	return returnValue;
}

void Core::exportData(int flags)	//SLOT
{
	/* FLAGS:
	actuationLevel = 0x1
	position X, Y, Z = 0x2
	orientaion X, Y, Z = 0x4
	DH parameters = 0x8
	*/
		
	QString filename = QFileDialog::getSaveFileName(0, tr("Export File As"), ".", "");
	
	exportFile = new QFile(filename);
	
	if (exportFile->open(QIODevice::WriteOnly))
	{
		QByteArray singleLine;
		QByteArray number;
		
		for (int i=0; i < robot->jointList.size(); i++)
		{
			singleLine.clear();
			singleLine.append(number.setNum(i));
			singleLine.append(QString(" "));
			
			switch (robot->jointList[i].getType())
			{
				case 1:	
					singleLine.append("Revolute");
					break;
				case 2: 
					singleLine.append("Prismatic");
					break;
				case 3:
					singleLine.append("Gripper");
					break;
				default:
					singleLine.append("Joint");
					break;
			}
			
			if (flags & 0x1)
			{
				singleLine.append(QString(" "));
				singleLine.append(number.setNum(round(robot->jointList[i].getCurrentActuation())));
			}
			
			if (flags & 0x2)
			{
				singleLine.append(QString(" "));
				singleLine.append(number.setNum(round(robot->jointList[i].getCenter().x())));
				singleLine.append(QString(" "));
				
				singleLine.append(number.setNum(round(robot->jointList[i].getCenter().y())));
				singleLine.append(QString(" "));
				
				singleLine.append(number.setNum(round(robot->jointList[i].getCenter().z())));
			}
			
			if (flags & 0x4)
			{
				singleLine.append(QString(" "));
				singleLine.append(number.setNum(round(robot->jointList[i].orientationK().x())));
				singleLine.append(QString(" "));
				
				singleLine.append(number.setNum(round(robot->jointList[i].orientationK().y())));
				singleLine.append(QString(" "));
				
				singleLine.append(number.setNum(round(robot->jointList[i].orientationK().z())));
			}
			
			if (flags & 0x8)
			{
				singleLine.append(QString(" "));
				singleLine.append(number.setNum(round(robot->jointList[i].dhTranslationK())));
				singleLine.append(QString(" "));
				
				singleLine.append(number.setNum(round(robot->jointList[i].dhRotationK())));
				singleLine.append(QString(" "));
				
				singleLine.append(number.setNum(round(robot->jointList[i].dhTranslationI())));
				singleLine.append(QString(" "));
				
				singleLine.append(number.setNum(round(robot->jointList[i].dhRotationI())));
			}
			singleLine.append("\n");
			exportFile->write(singleLine);
		}
	}
	else
	{
		QMessageBox error;
		error.setText(QString(tr("Unable to open file %1")).arg(filename));
		error.exec();
	}
	
	exportFile->close();
}

bool Core::loadRobotFromFile(QString filename)
{
    // Read data from file and save it to arrays
    // Each row in the matrix includes:
    // link number, joint type, translation along k, rotation around k, translation along i, rotation around i, min actuation, max actuation

//This really should be done using Qt's file functions/interface

    FILE *file;
    char oneline[255];
    int NumberOfJoints = 0;
    double output[3][4] = {0.0};
    int NumberOfAnimations;
    vector3D up(0.0, 1.0, 0.0);


    robot->reset();

    file = fopen(filename.toStdString().c_str(), "rt"); // .rbt file
      if (file!=NULL)
    {

        readstr(file, oneline);
        robot->setDescription(oneline);//Robot's description
//qDebug()<<"Description read is"<<oneline;
        readstr(file,oneline);
       
        sscanf(oneline, "%d\n", &NumberOfJoints);
	Last_Link = NumberOfJoints;

//qDebug()<<"Number of joints is"<<NumberOfJoints;
	
        for(int i = 0; i < NumberOfJoints; i++){

            readstr(file, oneline);
            //qDebug()<<"Robot read line is"<<oneline;
            sscanf(oneline, "%c %lf %lf %lf %lf %lf %lf\n",
                    &type[i],	//Revolute, Prismatic Or Gripper
                    &DHList[i][0],	//? DH parameter
                    &DHList[i][1],	//d DH parameter
                    &DHList[i][2],	//a DH parameter
                    &DHList[i][3],	//a DH parameter
                    &Limits[i][0],	// Lower limit
                    &Limits[i][1]);	// Upper limit

            makeTransformMatrixFromDHParams(i);

            Q[i] = 0.0;
            absolute_Params[i][0] = output[0][0];   absolute_Params[i][1] = output[1][0];   absolute_Params[i][2] = output[2][0];
            absolute_Params[i][3] = output[0][1];   absolute_Params[i][4] = output[1][1];   absolute_Params[i][5] = output[2][1];
            absolute_Params[i][6] = output[0][2];   absolute_Params[i][7] = output[1][2];   absolute_Params[i][8] = output[2][2];
            absolute_Params[i][9] = output[0][3];   absolute_Params[i][10] = output[1][3];  absolute_Params[i][11] = output[2][3];

            if (i != 0){
                switch (type[i]) {
                    case 'r':
                        robot->addJoint(Joint(REVOLUTE));
                        robot->jointList[i].setCenter(findCenter(i, output[0][0]));
                        robot->jointList[i].setStaticCenter(findCenter(i, output[0][0]));//used for drawing
                        robot->jointList[i].setRotationLimits(Limits[i][0],Limits[i][1]);
                        robot->jointList[i].setOrientationK(findKAxisOrientation(output[0][0]));
                        robot->jointList[i].setStaticOrientationK(findKAxisOrientation(output[0][0]));//for drawing
                        break;
                        
                   case 'p':
                        robot->addJoint(Joint(PRISMATIC));
                        robot->jointList[i].setCenter(findCenter(i, output[0][0]));
                        robot->jointList[i].setStaticCenter(findCenter(i, output[0][0]));//used for drawing
                        robot->jointList[i].setExtensionLimits(Limits[i][0],Limits[i][1]);
                        robot->jointList[i].setOrientationK(findKAxisOrientation(output[0][0]));
                        robot->jointList[i].setStaticOrientationK(findKAxisOrientation(output[0][0]));//for drawing
                        qDebug()<< i <<" Joint's center is:"<< findCenter(i, output[0][0]).x() << findCenter(i, output[0][0]).y() << findCenter(i, output[0][0]).z();
                        break;
                        
                   case 'g':
                        robot->addJoint(Joint(GRIPPER));
                        robot->jointList[i].setCenter(findCenter(i, output[0][0]));
                        robot->jointList[i].setStaticCenter(coord3D(output[0][0],output[1][0], output[2][0]));//used for drawing
                        robot->jointList[i].setOrientationK(findKAxisOrientation(output[0][0]));//for drawing
                        robot->jointList[i].setStaticOrientationK(findKAxisOrientation(output[0][0]));
                        break;
                    }
                }
                else{
                    switch (type[i]) {
                        case 'r':
                            robot->addJoint(Joint(REVOLUTE));
                            robot->jointList[i].setCenter(coord3D(0,0,0));
                            robot->jointList[i].setStaticCenter(coord3D(0,0,0));//used for drawing
                            robot->jointList[i].setRotationLimits(Limits[i][0],Limits[i][1]);
                            robot->jointList[i].setOrientationK(vector3D(0, 0, 1));
                            robot->jointList[i].setStaticOrientationK(vector3D(0, 0, 1));//for drawing
                            break;
                            
                       case 'p':
                            robot->addJoint(Joint(PRISMATIC));
                            robot->jointList[i].setCenter(coord3D(0,0,0));
                            robot->jointList[i].setStaticCenter(coord3D(0,0,0));//used for drawing
                            robot->jointList[i].setExtensionLimits(Limits[i][0],Limits[i][1]);
                            robot->jointList[i].setOrientationK(vector3D(0, 0, 1));
                            robot->jointList[i].setStaticOrientationK(vector3D(0, 0, 1));//for drawing
                            break;
                    }
                }
            }
        //READ Animation data
        readstr(file,oneline);
        sscanf(oneline, "%d\n", &NumberOfAnimations);
        
        Animation *tempAnimation;

        for(int i = 0; i < NumberOfAnimations; i++)
        {

		readstr(file,oneline);
		sscanf(oneline, "%d %lf %lf %d\n",
                    &animationJoint,
                    &animationStart,
                    &animationStop,
                    &animationPriority);
		
		tempAnimation = new Animation(animationJoint, animationStart, animationStop, animationPriority);
		
		robot->addAnimation(tempAnimation);
        }

        fclose (file);

        emit robotLoaded();

        return true;
    	}
	else
	{
	return false;
	}
}



////////////////////////////////
//// Matrix Math Functions ////
//////////////////////////////


void Core::Copy_Matrix(double *A, double *B, int nrows, int ncols)
// http://mymathlib.webtrellis.net/matrices/datamovement/copy_matrix.html

{
////////////////////////////////////////////////////////////////////////////////
//  void Copy_Matrix(double *A, double *B, int nrows, int ncols)              //
//                                                                            //
//  Description:                                                              //
//     Copy the nrows x ncols matrix B to the nrows x ncols matrix A.         //
//     i.e.    A = B.                                                         //
//                                                                            //
//  Arguments:                                                                //
//     double *A    Pointer to the first element of the destination matrix    //
//                  A[nrows][ncols].                                          //
//     double *B    Pointer to the first element of the source matrix         //
//                  B[nrows][ncols].                                          //
//     int    nrows The number of rows matrices A and B.                      //
//     int    ncols The number of columns of the matrices A and B.            //
//                                                                            //
//  Return Values:                                                            //
//     void                                                                   //
//                                                                            //
//  Example:                                                                  //
//     #define N                                                              //
//     #define M                                                              //
//     double A[N][M],  B[N][M];                                              //
//                                                                            //
//     (your code to initialize the matrix B)                                 //
//                                                                            //
//     Copy_Matrix(&A[0][0], &B[0][0], N, M);                                 //
//     printf(" Matrix A is \n");                                             //
////////////////////////////////////////////////////////////////////////////////
    memcpy(A, B, sizeof(double) * nrows * ncols);

}

void Core::Transpose_Matrix(double *At, double *A, int nrows, int ncols)
//http://mymathlib.webtrellis.net/matrices/datamovement/transposing.html
{
////////////////////////////////////////////////////////////////////////////////
//  void Transpose_Matrix(double *At, double *A, int nrows, int ncols)        //
//                                                                            //
//  Description:                                                              //
//     Take the transpose of A and store in At, i.e. At = A'.                 //
//     The matrix At should be declared as double At[ncols][nrows] in the     //
//     calling routine, and the matrix A declared as double A[nrows[ncols].   //
//     In general, At and A should be disjoint i.e. their memory locations    //
//     should be distinct.                                                    //
//                                                                            //
//  Arguments:                                                                //
//     double *At   Pointer to the first element of the matrix At.            //
//     double *A    Pointer to the first element of the matrix A.             //
//     int    nrows The number of rows of matrix A and number of columns of   //
//                  the matrix At.                                            //
//     int    ncols The number of columns of the matrix A and the number of   //
//                  rows of the matrix At.                                    //
//                                                                            //
//  Return Values:                                                            //
//     void                                                                   //
//                                                                            //
//  Example:                                                                  //
//     #define N                                                              //
//     #define M                                                              //
//     double A[M][N],  At[N][M];                                             //
//                                                                            //
//     (your code to initialize the matrix A)                                 //
//                                                                            //
//     Transpose_Matrix(&At[0][0], &A[0][0], M, N);                           //
//     printf("The transpose of A is the matrix At \n"); ...                  //
////////////////////////////////////////////////////////////////////////////////
   double *pA;
   double *pAt;
   int i = 0,j = 0;

   for (i = 0; i < nrows; At += 1, A += ncols, i++) {
      pAt = At;
      pA = A;
      for (j = 0; j < ncols; pAt += nrows, j++) *pAt = pA[j];
   }
}

void Core::Multiply_Matrices(double *C, double *A, int nrows, int ncols, double *B, int mcols)
// http://mymathlib.webtrellis.net/matrices/arithmetic/mul_mat.html
{
////////////////////////////////////////////////////////////////////////////////
//  void Multiply_Matrices(double *C, double *A, int nrows, int ncols,        //
//                                                    double *B, int mcols)   //
//                                                                            //
//  Description:                                                              //
//     Post multiply the nrows x ncols matrix A by the ncols x mcols matrix   //
//     B to form the nrows x mcols matrix C, i.e. C = A B.                    //
//     The matrix C should be declared as double C[nrows][mcols] in the       //
//     calling routine.  The memory allocated to C should not include any     //
//     memory allocated to A or B.                                            //
//                                                                            //
//  Arguments:                                                                //
//     double *C    Pointer to the first element of the matrix C.             //
//     double *A    Pointer to the first element of the matrix A.             //
//     int    nrows The number of rows of the matrices A and C.               //
//     int    ncols The number of columns of the matrices A and the           //
//                   number of rows of the matrix B.                          //
//     double *B    Pointer to the first element of the matrix B.             //
//     int    mcols The number of columns of the matrices B and C.            //
//                                                                            //
//  Return Values:                                                            //
//     void                                                                   //
//                                                                            //
//  Example:                                                                  //
//     #define N                                                              //
//     #define M                                                              //
//     #define NB                                                             //
//     double A[M][N],  B[N][NB], C[M][NB];                                   //
//                                                                            //
//     (your code to initialize the matrices A and B)                         //
//                                                                            //
//     Multiply_Matrices(&C[0][0], &A[0][0], M, N, &B[0][0], NB);             //
//     printf("The matrix C is \n"); ...                                      //
////////////////////////////////////////////////////////////////////////////////
   double *pA = A;
   double *pB;
   double *p_B;
   double *pC = C;
   int i,j,k;

   for (i = 0; i < nrows; A += ncols, i++)
      for (p_B = B, j = 0; j < mcols; C++, p_B++, j++) {
         pB = p_B;
         pA = A;
         *C = 0.0;
         for (k = 0; k < ncols; pA++, pB += mcols, k++)
            *C += *pA * *pB;
      }
}

void Core::Jacobi_Cyclic_Method(double eigenvalues[], double *eigenvectors, double *A, int n)
// http://mymathlib.webtrellis.net/matrices/eigen/symmetric.html
{
    ////////////////////////////////////////////////////////////////////////////////
//  void Jacobi_Cyclic_Method                                                 //
//            (double eigenvalues[], double *eigenvectors, double *A, int n)  //
//                                                                            //
//  Description:                                                              //
//     Find the eigenvalues and eigenvectors of a symmetric n x n matrix A    //
//     using the Jacobi method. Upon return, the input matrix A will have     //
//     been modified.                                                         //
//     The Jacobi procedure for finding the eigenvalues and eigenvectors of a //
//     symmetric matrix A is based on finding a similarity transformation     //
//     which diagonalizes A.  The similarity transformation is given by a     //
//     product of a sequence of orthogonal (rotation) matrices each of which  //
//     annihilates an off-diagonal element and its transpose.  The rotation   //
//     effects only the rows and columns containing the off-diagonal element  //
//     and its transpose, i.e. if a[i][j] is an off-diagonal element, then    //
//     the orthogonal transformation rotates rows a[i][] and a[j][], and      //
//     equivalently it rotates columns a[][i] and a[][j], so that a[i][j] = 0 //
//     and a[j][i] = 0.                                                       //
//     The cyclic Jacobi method considers the off-diagonal elements in the    //
//     following order: (0,1),(0,2),...,(0,n-1),(1,2),...,(n-2,n-1).  If the  //
//     the magnitude of the off-diagonal element is greater than a treshold,  //
//     then a rotation is performed to annihilate that off-diagnonal element. //
//     The process described above is called a sweep.  After a sweep has been //
//     completed, the threshold is lowered and another sweep is performed     //
//     with the new threshold. This process is completed until the final      //
//     sweep is performed with the final threshold.                           //
//     The orthogonal transformation which annihilates the matrix element     //
//     a[k][m], k != m, is Q = q[i][j], where q[i][j] = 0 if i != j, i,j != k //
//     i,j != m and q[i][j] = 1 if i = j, i,j != k, i,j != m, q[k][k] =       //
//     q[m][m] = cos(phi), q[k][m] = -sin(phi), and q[m][k] = sin(phi), where //
//     the angle phi is determined by requiring a[k][m] -> 0.  This condition //
//     on the angle phi is equivalent to                                      //
//               cot(2 phi) = 0.5 * (a[k][k] - a[m][m]) / a[k][m]             //
//     Since tan(2 phi) = 2 tan(phi) / (1.0 - tan(phi)^2),                    //
//               tan(phi)^2 + 2cot(2 phi) * tan(phi) - 1 = 0.                 //
//     Solving for tan(phi), choosing the solution with smallest magnitude,   //
//       tan(phi) = - cot(2 phi) + sgn(cot(2 phi)) sqrt(cot(2phi)^2 + 1).     //
//     Then cos(phi)^2 = 1 / (1 + tan(phi)^2) and sin(phi)^2 = 1 - cos(phi)^2 //
//     Finally by taking the sqrts and assigning the sign to the sin the same //
//     as that of the tan, the orthogonal transformation Q is determined.     //
//     Let A" be the matrix obtained from the matrix A by applying the        //
//     similarity transformation Q, since Q is orthogonal, A" = Q'AQ, where Q'//
//     is the transpose of Q (which is the same as the inverse of Q).  Then   //
//         a"[i][j] = Q'[i][p] a[p][q] Q[q][j] = Q[p][i] a[p][q] Q[q][j],     //
//     where repeated indices are summed over.                                //
//     If i is not equal to either k or m, then Q[i][j] is the Kronecker      //
//     delta.   So if both i and j are not equal to either k or m,            //
//                                a"[i][j] = a[i][j].                         //
//     If i = k, j = k,                                                       //
//        a"[k][k] =                                                          //
//           a[k][k]*cos(phi)^2 + a[k][m]*sin(2 phi) + a[m][m]*sin(phi)^2     //
//     If i = k, j = m,                                                       //
//        a"[k][m] = a"[m][k] = 0 =                                           //
//           a[k][m]*cos(2 phi) + 0.5 * (a[m][m] - a[k][k])*sin(2 phi)        //
//     If i = k, j != k or m,                                                 //
//        a"[k][j] = a"[j][k] = a[k][j] * cos(phi) + a[m][j] * sin(phi)       //
//     If i = m, j = k, a"[m][k] = 0                                          //
//     If i = m, j = m,                                                       //
//        a"[m][m] =                                                          //
//           a[m][m]*cos(phi)^2 - a[k][m]*sin(2 phi) + a[k][k]*sin(phi)^2     //
//     If i= m, j != k or m,                                                  //
//        a"[m][j] = a"[j][m] = a[m][j] * cos(phi) - a[k][j] * sin(phi)       //
//                                                                            //
//     If X is the matrix of normalized eigenvectors stored so that the ith   //
//     column corresponds to the ith eigenvalue, then AX = X Lamda, where     //
//     Lambda is the diagonal matrix with the ith eigenvalue stored at        //
//     Lambda[i][i], i.e. X'AX = Lambda and X is orthogonal, the eigenvectors //
//     are normalized and orthogonal.  So, X = Q1 Q2 ... Qs, where Qi is      //
//     the ith orthogonal matrix,  i.e. X can be recursively approximated by  //
//     the recursion relation X" = X Q, where Q is the orthogonal matrix and  //
//     the initial estimate for X is the identity matrix.                     //
//     If j = k, then x"[i][k] = x[i][k] * cos(phi) + x[i][m] * sin(phi),     //
//     if j = m, then x"[i][m] = x[i][m] * cos(phi) - x[i][k] * sin(phi), and //
//     if j != k and j != m, then x"[i][j] = x[i][j].                         //
//                                                                            //
//  Arguments:                                                                //
//     double  eigenvalues                                                    //
//        Array of dimension n, which upon return contains the eigenvalues of //
//        the matrix A.                                                       //
//     double* eigenvectors                                                   //
//        Matrix of eigenvectors, the ith column of which contains an         //
//        eigenvector corresponding to the ith eigenvalue in the array        //
//        eigenvalues.                                                        //
//     double* A                                                              //
//        Pointer to the first element of the symmetric n x n matrix A. The   //
//        input matrix A is modified during the process.                      //
//     int     n                                                              //
//        The dimension of the array eigenvalues, number of columns and rows  //
//        of the matrices eigenvectors and A.                                 //
//                                                                            //
//  Return Values:                                                            //
//     Function is of type void.                                              //
//                                                                            //
//  Example:                                                                  //
//     #define N                                                              //
//     double A[N][N], double eigenvalues[N], double eigenvectors[N][N]       //
//                                                                            //
//     (your code to initialize the matrix A )                                //
//                                                                            //
//     Jacobi_Cyclic_Method(eigenvalues, (double*)eigenvectors,               //
//                                                          (double *) A, N); //
////////////////////////////////////////////////////////////////////////////////

   int row, i, j, k, m;
   double *pAk, *pAm, *p_r, *p_e;
   double threshold_norm;
   double threshold;
   double tan_phi, sin_phi, cos_phi, tan2_phi, sin2_phi, cos2_phi;
   double sin_2phi, cos_2phi, cot_2phi;
   double dum1;
   double dum2;
   double dum3;
   double r;
   double max;

                  // Take care of trivial cases

   if ( n < 1) return;
   if ( n == 1) {
      eigenvalues[0] = *A;
      *eigenvectors = 1.0;
      return;
   }

          // Initialize the eigenvalues to the identity matrix.

   for (p_e = eigenvectors, i = 0; i < n; i++)
      for (j = 0; j < n; p_e++, j++)
         if (i == j) *p_e = 1.0; else *p_e = 0.0;

            // Calculate the threshold and threshold_norm.

   for (threshold = 0.0, pAk = A, i = 0; i < ( n - 1 ); pAk += n, i++)
      for (j = i + 1; j < n; j++) threshold += *(pAk + j) * *(pAk + j);
   threshold = sqrt(threshold + threshold);
   threshold_norm = threshold * DBL_EPSILON;
   max = threshold + 1.0;
   while (threshold > threshold_norm) {
      threshold /= 10.0;
      if (max < threshold) continue;
      max = 0.0;
      for (pAk = A, k = 0; k < (n-1); pAk += n, k++) {
         for (pAm = pAk + n, m = k + 1; m < n; pAm += n, m++) {
            if ( fabs(*(pAk + m)) < threshold ) continue;

                 // Calculate the sin and cos of the rotation angle which
                 // annihilates A[k][m].

            cot_2phi = 0.5 * ( *(pAk + k) - *(pAm + m) ) / *(pAk + m);
            dum1 = sqrt( cot_2phi * cot_2phi + 1.0);
            if (cot_2phi < 0.0) dum1 = -dum1;
            tan_phi = -cot_2phi + dum1;
            tan2_phi = tan_phi * tan_phi;
            sin2_phi = tan2_phi / (1.0 + tan2_phi);
            cos2_phi = 1.0 - sin2_phi;
            sin_phi = sqrt(sin2_phi);
            if (tan_phi < 0.0) sin_phi = - sin_phi;
            cos_phi = sqrt(cos2_phi);
            sin_2phi = 2.0 * sin_phi * cos_phi;
            cos_2phi = cos2_phi - sin2_phi;

                     // Rotate columns k and m for both the matrix A
                     //     and the matrix of eigenvectors.

            p_r = A;
            dum1 = *(pAk + k);
            dum2 = *(pAm + m);
            dum3 = *(pAk + m);
            *(pAk + k) = dum1 * cos2_phi + dum2 * sin2_phi + dum3 * sin_2phi;
            *(pAm + m) = dum1 * sin2_phi + dum2 * cos2_phi - dum3 * sin_2phi;
            *(pAk + m) = 0.0;
            *(pAm + k) = 0.0;
            for (i = 0; i < n; p_r += n, i++) {
               if ( (i == k) || (i == m) ) continue;
               if ( i < k ) dum1 = *(p_r + k); else dum1 = *(pAk + i);
               if ( i < m ) dum2 = *(p_r + m); else dum2 = *(pAm + i);
               dum3 = dum1 * cos_phi + dum2 * sin_phi;
               if ( i < k ) *(p_r + k) = dum3; else *(pAk + i) = dum3;
               dum3 = - dum1 * sin_phi + dum2 * cos_phi;
               if ( i < m ) *(p_r + m) = dum3; else *(pAm + i) = dum3;
            }
            for (p_e = eigenvectors, i = 0; i < n; p_e += n, i++) {
               dum1 = *(p_e + k);
               dum2 = *(p_e + m);
               *(p_e + k) = dum1 * cos_phi + dum2 * sin_phi;
               *(p_e + m) = - dum1 * sin_phi + dum2 * cos_phi;
            }
         }
         for (i = 0; i < n; i++)
            if ( i == k ) continue;
            else if ( max < fabs(*(pAk + i))) max = fabs(*(pAk + i));
      }
   }
   for (pAk = A, k = 0; k < n; pAk += n, k++) eigenvalues[k] = *(pAk + k);
}

double Core::pythag(double a, double b)
//http://www.nrbook.com/a/bookcpdf.php
        // Section 2.6
{
    //Computes (a2 + b2)1=2 without destructive underflow or overflow.
    double absa,absb;
    absa=fabs(a);
    absb=fabs(b);
    if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa));
    else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
}

void Core::svdcmp(double **a, int m, int n, double w[], double **v)
//http://www.nrbook.com/a/bookcpdf.php
        // Section 2.6
{
    // Given matrix a[1..m][1..n], this routine computes its singular value decomposition,
    // A = U*W*V_transpose. The matrix U replaces a on output. The diagonal matrix of singular
    // values W is output as a vector w[1..n]. The matrix V (not the transpose of V) is output as v[1..n][1..n]

int flag,i,its,j,jj,k,l,nm;
double anorm,c,f,g,h,s,scale,x,y,z,*rv1;
rv1 = dvector(1,n);


// Householder reduction to bidiagonal form
g=scale=anorm=0.0;
for (i=0;i<n;i++) {
    l=i+1;
    rv1[i]=scale*g;
    g=s=scale=0.0;
    if (i < m) {
        for (k=i;k<m;k++) scale += fabs(a[k][i]);
        if (scale) {
            for (k=i;k<m;k++) {
                a[k][i] /= scale;
                s += a[k][i]*a[k][i];
            }
            f=a[i][i];
            g = -SIGN(sqrt(s),f);
            h=f*g-s;
            a[i][i]=f-g;
            for (j=0;j<n;j++) {
                for (s=0.0,k=i;k<m;k++) s += a[k][i]*a[k][j];
                f=s/h;
                for (k=i;k<m;k++) a[k][j] += f*a[k][i];
            }
            for (k=i;k<m;k++) a[k][i] *= scale;
        }
    }
    w[i]=scale *g;
    g=s=scale=0.0;
    if (i < m && i != n) {
        for (k=0;k<n;k++) scale += fabs(a[i][k]);
        if (scale) {
            for (k=0;k<n;k++) {
                a[i][k] /= scale;
                s += a[i][k]*a[i][k];
            }
            f=a[i][l];
            g = -SIGN(sqrt(s),f);
            h=f*g-s;
            a[i][l]=f-g;
            for (k=0;k<n;k++) rv1[k]=a[i][k]/h;
            for (j=0;j<m;j++) {
                for (s=0.0,k=0;k<n;k++) s += a[j][k]*a[i][k];
                for (k=0;k<n;k++) a[j][k] += s*rv1[k];
            }
            for (k=0;k<n;k++) a[i][k] *= scale;
        }
    }
    anorm=FMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
}

// Accumulation of right-hand transformations
for (i=(n-1);i>=0;i--) {
    if (i < n) {
        if (g) {
            for (j=0;j<n;j++) // Double division to avoid possible underflow.
            v[j][i]=(a[i][j]/a[i][l])/g;
            for (j=0;j<n;j++) {
                for (s=0.0,k=0;k<n;k++) s += a[i][k]*v[k][j];
                for (k=0;k<n;k++) v[k][j] += s*v[k][i];
            }
        }
        for (j=0;j<n;j++) v[i][j]=v[j][i]=0;
    }
    v[i][i] = 1.0;
    g=rv1[i];
    l=i;
}

// Accumulation of left-hand transformations.
for (i=IMIN(m,n);i>=0;i--) {
    l=i+1;
    g=w[i];
    for (j=0;j<n;j++) a[i][j]=0.0;
    if (g) {
        g=1.0/g;
        for (j=0;j<n;j++) {
        for (s=0.0,k=0;k<m;k++) s += a[k][i]*a[k][j];
        f=(s/a[i][i])*g;
        for (k=i;k<m;k++) a[k][j] += f*a[k][i];
        }
        for (j=i;j<m;j++) a[j][i] *= g;
    } else for (j=i;j<m;j++) a[j][i]=0.0;
    ++a[i][i];
}

// Diagonalization of the bidiagonal form: Loop over singular values, and over allowed iterations.
for (k=n;k>=0;k--) {
    for (its=0;its<30;its++) {
        flag=1;
        for (l=k;l>=0;l--) { //Test for splitting.
            nm=l-1; //Note that rv1[1] is always zero.
            if ((double)(fabs(rv1[l])+anorm) == anorm) {
                flag=0;
                break;
            }
            if ((double)(fabs(w[nm])+anorm) == anorm) break;
        }
        if (flag) {
            c=0.0; //Cancellation of rv1[l], if l > 1.
            s=1.0;
            for (i=0;i<=k;i++) {
                f=s*rv1[i];
                rv1[i]=c*rv1[i];
                if ((double)(fabs(f)+anorm) == anorm) break;
                g=w[i];
                h = pythag(f,g);
                w[i]=h;
                h=1.0/h;
                c=g*h;
                s = -f*h;
                for (j=0;j<m;j++) {
                    y=a[j][nm];
                    z=a[j][i];
                    a[j][nm]=y*c+z*s;
                    a[j][i]=z*c-y*s;
                }
            }
        }
        z=w[k];
        if (l == k) { //Convergence.
            if (z < 0.0) { //Singular value is made nonnegative.
                w[k] = -z;
                for (j=0;j<n;j++) v[j][k] = -v[j][k];
            }
            break;
        }
        if (its == 30) nrerror("no convergence in 30 svdcmp iterations");
        x=w[l]; //Shift from bottom 2-by-2 minor.
        nm=k-1;
        y=w[nm];
        g=rv1[nm];
        h=rv1[k];
        f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
        g=pythag(f,1.0);
        f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
        c=s=1.0; //Next QR transformation:
        for (j=0;j<nm;j++) {
            i=j+1;
            g=rv1[i];
            y=w[i];
            h=s*g;
            g=c*g;
            z=pythag(f,h);
            rv1[j]=z;
            c=f/z;
            s=h/z;
            f=x*c+g*s;
            g = g*c-x*s;
            h=y*s;
            y *= c;
            for (jj=0;jj<n;jj++) {
                x=v[jj][j];
                z=v[jj][i];
                v[jj][j]=x*c+z*s;
                v[jj][i]=z*c-x*s;
            }
            z=pythag(f,h);
            w[j]=z; //Rotation can be arbitrary if z = 0.
            if (z) {
                z=1.0/z;
                c=f*z;
                s=h*z;
            }
            f=c*g+s*y;
            x=c*y-s*g;
            for (jj=0;jj<m;jj++) {
                y=a[jj][j];
                z=a[jj][i];
                a[jj][j]=y*c+z*s;
                a[jj][i]=z*c-y*s;
        }
        }
        rv1[l]=0.0;
        rv1[k]=f;
        w[k]=x;
    }
}
free_dvector(rv1,1,n);
}

void Core::Cross_Product(double &M1, double &M2, double &Cross)
{
    // Stores the solution of M1 X M2 in Cross
    // M1, M2, Cross = [3]

    // Example:
    // Cross_Product(Kaxis[0], Difference[0], Cross[0]);

    // Set up temporary variables
    double Matrix1[3] = {0}, Matrix2[3] = {0}, result[3] = {0};

    // Load Parameters into temporary variables
    Copy_Matrix(&Matrix1[0], &M1, 3, 1);
    Copy_Matrix(&Matrix2[0], &M2, 3, 1);

    ////////////////////////////
    // Perform Cross Product///
    //////////////////////////
    result[0] = (Matrix1[1]*Matrix2[2]) - (Matrix1[2]*Matrix2[1]);
    result[1] = (Matrix1[2]*Matrix2[0]) - (Matrix1[0]*Matrix2[2]);
    result[2] = (Matrix1[0]*Matrix2[1]) - (Matrix1[1]*Matrix2[0]);

    // Load result into Cross
    Copy_Matrix(&Cross, &result[0], 3, 1);
}

double Core::Get_Condition_Number(double &A, double &A_inv, int height, int width)
{
    // This function calculates the condition number of A to determine whether it is well conditioned

    // Parameters:
    // - A -> matrix to be checked
    // - A_inv -> the inverse of A
    // - height, width -> the height and width of A

    // Return:
    // - returns a double containing the condition number

    // Set up temp variables
    double matrix[6][7] = {0}, matrix_inv[6][7] = {0}; // Note: sizes must be fixed**************************************************************************
    double matrix_NORM = 0, matrix_inv_NORM = 0;

    // Load parameters into temp variables
    Copy_Matrix(&matrix[0][0], &A, height, width);
    Copy_Matrix(&matrix_inv[0][0], &A_inv, height, width);

    // Find the Norm
    matrix_NORM = Get_Norm(matrix[0][0], 6, 7);
    matrix_inv_NORM = Get_Norm(matrix_inv[0][0], 6, 7);

//     qDebug()<<"matrix_NORM is:  "<<matrix_NORM;
//     qDebug()<<"matrix_inv_NORM is:  "<<matrix_inv_NORM;

    return (matrix_NORM * matrix_inv_NORM);
}

double Core::Get_Norm(double &A, int height, int width)
{
    // This function takes a matrix and its height and width, and returns its magnitude as a double.

    // Set up temp variables
    double matrix[6][7] = {0};          // Note: sizes must be fixed**************************************************************************
    double NORM = 0, temp[6] = {0};

    // Load parameters into temp variables
    Copy_Matrix(&matrix[0][0], &A, height, width);

    // Compute the Row Summs
    for (int g = 0; g < height; g++)
    {
        for (int f = 0; f < width; f++)
        {
            temp[g] = temp[g] + fabs(matrix[g][f]);
        }
    }

    // Find the max row sum
    NORM = temp[0];
    for (int r = 1; r < height; r++)
    {
        if (temp[r] > NORM){
            NORM = temp[r];
        }
    }

    return(NORM);
}


///////////////////////////
//// Kinematics Setup ////
/////////////////////////


void Core::makeTransformMatrixFromDHParams(int jointNumber)
{

modifyTransformMatrix(jointNumber, 0,0);

}

void Core::modifyTransformMatrix(int jointNumber, double Rotation, double Extension)
{
//fake_modifyJoints(jointNumber, Rotation, Extension);

    // Take Tmatrix and modify it to include current displacement data

    // Note:
    // Prismatic joints should have no rotation and Revolute joints should have no extension

    // Returns 4x4 matrix

    double Tmatrix[4][4] = {0.0}, Temp_1[4][4] = {0.0}, Temp_2[4][4] = {0.0};
    double Krotation[4][4] = {0.0}, Irotation[4][4] = {0.0}, Ktranslation[4][4] = {0.0}, Itranslation[4][4] = {0.0};
    const double PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;

// Create the tranformation standard matricies
    // Make rotation in K
    Krotation[0][0] = cos((DHList[jointNumber][0] + Rotation)/180*PI);
    Krotation[0][1] = -sin((DHList[jointNumber][0] + Rotation)/180*PI);
    Krotation[1][0] = sin((DHList[jointNumber][0] + Rotation)/180*PI);
    Krotation[1][1] = cos((DHList[jointNumber][0] + Rotation)/180*PI);
    Krotation[2][2] = 1.0;
    Krotation[3][3] = 1.0;

//     qDebug()<<"Krotation[0][1]"<<Krotation[0][1];

    // Make rotation in I
    Irotation[0][0] = 1.0;
    Irotation[1][1] = cos((DHList[jointNumber][3])/180*PI);
    Irotation[1][2] = -sin((DHList[jointNumber][3])/180*PI);
    Irotation[2][1] = sin((DHList[jointNumber][3])/180*PI);
    Irotation[2][2] = cos((DHList[jointNumber][3])/180*PI);
    Irotation[3][3] = 1.0;

    // Make translation in K
    Ktranslation[0][0] = 1.0;
    Ktranslation[1][1] = 1.0;
    Ktranslation[2][2] = 1.0;
    Ktranslation[3][3] = 1.0;
    Ktranslation[2][3] = DHList[jointNumber][1] + Extension;

    // Make translation in I
    Itranslation[0][0] = 1.0;
    Itranslation[1][1] = 1.0;
    Itranslation[2][2] = 1.0;
    Itranslation[3][3] = 1.0;
    Itranslation[0][3] = DHList[jointNumber][2];

// Multiply all the standard transformation matrices to get the joint transformation matrix
    Multiply_Matrices(&Temp_1[0][0], &Krotation[0][0], 4, 4, &Ktranslation[0][0], 4);
    Multiply_Matrices(&Temp_2[0][0], &Temp_1[0][0], 4, 4, &Itranslation[0][0], 4);
    Multiply_Matrices(&Tmatrix[0][0], &Temp_2[0][0], 4, 4, &Irotation[0][0], 4);

// Determine which Tmatrix to overwrite
    switch (jointNumber) {
        case 0: Copy_Matrix(&T1[0][0], &Tmatrix[0][0], 4, 4); break;
        case 1: Copy_Matrix(&T2[0][0], &Tmatrix[0][0], 4, 4); break;
        case 2: Copy_Matrix(&T3[0][0], &Tmatrix[0][0], 4, 4); break;
        case 3: Copy_Matrix(&T4[0][0], &Tmatrix[0][0], 4, 4); break;
        case 4: Copy_Matrix(&T5[0][0], &Tmatrix[0][0], 4, 4); break;
        case 5: Copy_Matrix(&T6[0][0], &Tmatrix[0][0], 4, 4); break;
        case 6: Copy_Matrix(&T7[0][0], &Tmatrix[0][0], 4, 4); break;
    }

}


/////////////////////////////
//// Forward Kinematics ////
///////////////////////////


void Core::recalculateJoints()
{
    bool ok;

// Set up local variables
    double output[3][4] = {0.0};

    for(int i = 0; i < Last_Link; i++){
        double (Extension) = robot->jointList[i].getExtension(&ok);
        double (Rotation) = robot->jointList[i].getRotation(&ok);

        //qDebug()<<"Extension"<<i<<"equals"<<Extension;
        //qDebug()<<"Rotation"<<i<<"equals"<<Rotation;

        if(Extension < 0){
            Extension = fabs(Extension);
        }
        if(Rotation < 0){
            Rotation = 360 + Rotation;
        }

    // Determine the appropriate type of joint and update it
        if (type[i] == 'r'){  // if revolute
            modifyTransformMatrix(i, Rotation, 0);    // Update the transformation matrix
            Q[i] = Rotation;
            //qDebug()<<"Rotation"<<i<<" succeded";
            //qDebug()<<"Joint"<<i<<" is"<<type[i];
        }
        else {   // else prismatic
            modifyTransformMatrix(i, 0, Extension);         // Update the transformation matrix
            Q[i] = Extension;
            //qDebug()<<"Extension"<<i<<" succeded";
            //qDebug()<<"Joint"<<i<<" is"<<type[i];
        }

        //qDebug()<<"Joint"<<i<<"equals"<<Q[i];

    // Update the calculated absolute parameters of all the downstream joints
        robot->jointList[i].setCenter(findCenter(i, output[0][0]));
//         qDebug()<<"Output"<<i<<"   "<<output[0][0]<<"   "<< output[1][0]<<"   "<< output[2][0];
        robot->jointList[i].setOrientationK(findKAxisOrientation(output[0][0]));
        absolute_Params[i][0] = output[0][0];   absolute_Params[i][1] = output[1][0];   absolute_Params[i][2] = output[2][0];
        absolute_Params[i][3] = output[0][1];   absolute_Params[i][4] = output[1][1];   absolute_Params[i][5] = output[2][1];
        absolute_Params[i][6] = output[0][2];   absolute_Params[i][7] = output[1][2];   absolute_Params[i][8] = output[2][2];
        absolute_Params[i][9] = output[0][3];   absolute_Params[i][10] = output[1][3];  absolute_Params[i][11] = output[2][3];

//         if (i == Last_Link - 1)
//         	qDebug()<<"output"<<i<<"center:   "<<output[0][0]<<"    "<< output[1][0]<<"    "<< output[2][0];
        //if(i == Last_Link - 1)qDebug()<<"output"<<i<<"orientation:   "<<output[0][3]<<"    "<< output[1][3]<<"    "<< output[2][3];
    }

	emit robotUpdated();
}

coord3D Core::findCenter(int jointNumber, double &output)
{

    // Multiply all the transformation matricies up to the joint specified and
    // extract the joint's position and reference frame. The output will be of
    // the form: [center[3]; Ivector[3]; Jvector[3]; Kvector[3]] (3X4 matrix)

    // Note:
    // The function assumes that the joint exists

    // Example:
    // findCenter(Link, output[0][0]);


    double Ttotal[4][4] = {0.0}, result[3][4] = {0.0};

    Ttotal[0][0] = 1; Ttotal[1][1] = 1; Ttotal[2][2] = 1; Ttotal[3][3] = 1;

    double Ttemp[4][4] = {0.0};
    double T[4][4] = {0.0};

    // Build the cumulitive Transformation matrix by matrix multiplying every Tmatrix upstream of the specified joint.
    for(int j=1; j <= jointNumber; j++)
    {
        // Determine which Tmatrix to use
        switch (j) {
            case 1: Copy_Matrix(&T[0][0], &T1[0][0], 4, 4); break;
            case 2: Copy_Matrix(&T[0][0], &T2[0][0], 4, 4); break;
            case 3: Copy_Matrix(&T[0][0], &T3[0][0], 4, 4); break;
            case 4: Copy_Matrix(&T[0][0], &T4[0][0], 4, 4); break;
            case 5: Copy_Matrix(&T[0][0], &T5[0][0], 4, 4); break;
            case 6: Copy_Matrix(&T[0][0], &T6[0][0], 4, 4); break;
            case 7: Copy_Matrix(&T[0][0], &T7[0][0], 4, 4); break;
        }

        // Multiply each Tmatrix by Ttotal

        Multiply_Matrices(&Ttemp[0][0], &Ttotal[0][0], 4, 4, &T[0][0], 4);
        Copy_Matrix(&Ttotal[0][0], &Ttemp[0][0], 4, 4);
    }

    // Extract the position information from the Ttotal
    result[0][0] = Ttotal[0][3];
    result[1][0] = Ttotal[1][3];
    result[2][0] = Ttotal[2][3];

    // Extract the rotation information from the Ttotal
    result[0][1] = Ttotal[0][0];
    result[1][1] = Ttotal[1][0];
    result[2][1] = Ttotal[2][0];
    result[0][2] = Ttotal[0][1];
    result[1][2] = Ttotal[1][1];
    result[2][2] = Ttotal[2][1];
    result[0][3] = Ttotal[0][2];
    result[1][3] = Ttotal[1][2];
    result[2][3] = Ttotal[2][2];

    //qDebug()<<"Result"<<jointNumber<<"   "<<result[0][0]<<"   "<< result[1][0]<<"   "<< result[2][0];

    Copy_Matrix(&output, &result[0][0],3,4);

    return (coord3D(result[0][0],result[1][0],result[2][0]));

}

vector3D Core::findKAxisOrientation(double &output)
{
    // This function requires output to be set by findCenter() above

    double norm[3] = {0.0}, length = 0.0, temp[3][4] = {0.0};

    Copy_Matrix(&temp[0][0], &output,3,4);

    length = sqrt((temp[0][3] * temp[0][3]) + (temp[1][3] * temp[1][3]) + (temp[2][3] * temp[2][3]));

    norm[0] = temp[0][3] / length;
    norm[1] = temp[1][3] / length;
    norm[2] = temp[2][3] / length;

    return (vector3D(norm[0], norm[1], norm[2]));
    //return (vector3D(temp[0][3], temp[1][3], temp[2][3]));

}


/////////////////////////////
//// Inverse Kinematics ////
///////////////////////////


int Core::inverseKinematics(double desired_gripper_pos[3], double desired_gripper_frame_i[3], double desired_gripper_frame_j[3], double desired_gripper_frame_k[3])
{
// This function takes the end effector position and reference frame with respect to
// the base frame as parameters and returns a 7-by-6 array if the parameters are ok.
// Each row of the array represents a the position and frame of a joint. If the robot
// has less than 7 joints, the bottom rows will be padded with zeros.


// Set up Variables
    const int m = 6, n = 7;
    double E[3] = {0.0}, actual_gripper_pos[3] = {0.0}, center[3][4] = {0.0};
    double Cd[3][3] = {0.0}, Ci[3][3] = {0.0}, Ci_T[3][3] = {0.0}, Cdi[3][3] = {0.0};
    double error[6] = {0.0};
    double eigenvalues[3] = {0.0}, eigenvectors[3][3] = {0.0}, eigenvector[3] = {0.0};
    double Jacobian_inv[m][n] = {0.0}, condition_Number = 0, delta_Q[6] = {0.0}, w[n]= {0.0}, *v[n][n], ww[n][n] = {0.0}, vv[n][n] = {0.0}, vvww[n][n] = {0.0}, U_cnj[m][n] = {0.0};
    const double threshold = 1000;
    double Temp_J[m][n] = {0.0}, *Jacobian_pnt[m][n];
    int eig = 0;

// Top part of the error matrix
    // Find the current position
    findCenter(Last_Link, center[0][0]);
    actual_gripper_pos[0] = center[0][0];
    actual_gripper_pos[1] = center[1][0];
    actual_gripper_pos[2] = center[2][0];

    // Find the error
    E[0] = desired_gripper_pos[0] - actual_gripper_pos[0];
    E[1] = desired_gripper_pos[1] - actual_gripper_pos[1];
    E[2] = desired_gripper_pos[2] - actual_gripper_pos[2];


// Bottom part of the error matrix
    // Fill in the entries of the desired reference frame
    Cd[0][0] = desired_gripper_frame_i[0]; Cd[1][0] = desired_gripper_frame_i[1]; Cd[2][0] = desired_gripper_frame_i[2];
    Cd[0][1] = desired_gripper_frame_j[0]; Cd[1][1] = desired_gripper_frame_j[1]; Cd[2][1] = desired_gripper_frame_j[2];
    Cd[0][2] = desired_gripper_frame_k[0]; Cd[1][2] = desired_gripper_frame_k[1]; Cd[2][2] = desired_gripper_frame_k[2];

    // Fill in the entries of the actual reference frame
    Ci[0][0] = center[0][0]; Ci[1][0] = center[1][0]; Ci[2][0] = center[2][0];
    Ci[0][1] = center[0][1]; Ci[1][1] = center[1][1]; Ci[2][1] = center[2][1];
    Ci[0][2] = center[0][2]; Ci[1][2] = center[1][2]; Ci[2][2] = center[2][2];

    // Multiply Cd X Ci(transpose) = Cdi
    Transpose_Matrix(&Ci_T[0][0], &Ci[0][0], 3, 3);
    Multiply_Matrices(&Cdi[0][0], &Cd[0][0], 3, 3, &Ci_T[0][0], 3);

    // Find the axis of rotation of Cdi (eigenvector)
    Jacobi_Cyclic_Method(eigenvalues, (double*) eigenvectors, (double*) Cdi, 3);
    // 3 eigenvectors are returnes... Choose the real valued one.
    for(int y = 0; y < 3; y++){
        if ((eigenvectors[0][y] >= 0)&&(eigenvectors[1][y] >= 0)&&(eigenvectors[2][y] >= 0)){
            eig = y;
        }
    }
    eigenvector[0] = eigenvectors[0][eig];
    eigenvector[1] = eigenvectors[1][eig];
    eigenvector[2] = eigenvectors[2][eig];

// Join the top and bottom in 1 matrix
    error[0] = E[0];    error[1] = E[1];    error[2] = E[2];
    error[3] = eigenvector[0];  error[4] = eigenvector[1];  error[5] = eigenvector[2];


// Invert the Jacobian
    makeJacobian();
    Copy_Matrix(&Temp_J[0][0], &Jacobian[0][0], m, n);
    // Set the pointer reference
    for(int e = 0; e < m; e++){
        for(int f = 0; f < n; f++){
            Jacobian_pnt[e][f] = &Temp_J[e][f];
        }
    }
    // Set the pointer reference
    for(int e = 0; e < n; e++){
        for(int f = 0; f < n; f++){
            v[e][f] = &vv[e][f];
        }
    }
// Test****************************************************************************************************
    //int numb = 0;
    for(int e = 0; e < m; e++){
        for(int f = 0; f < n; f++){
            Temp_J[e][f] = 1;
            //numb++;
        }
    }

// Test end************************************************************************************************

    // Use single value decomposition
    svdcmp(&Jacobian_pnt[0][0], m, n, &w[0], &v[0][0]);
//     qDebug()<<"U[0][0] equals:  "<< Temp_J[0][0];
//     qDebug()<<"W[0] equals:  "<< w[0];
//     qDebug()<<"VV[0][0] equals:  "<< vv[0][0];

    // Invert W
    for(int q = 0; q < n; q++){
        ww[q][q] = 1 / w[q];
    }

    double V_T[n][n] = {0.0}, V_Tww[n][n] = {0.0}, A[n][n] = {0.0};
    Transpose_Matrix(&V_T[0][0], &vv[0][0], n, n);
    Multiply_Matrices(&V_Tww[0][0], &ww[0][0], n, n, &V_T[0][0], n);
    Multiply_Matrices(&A[0][0], &Temp_J[0][0], m, n, &V_Tww[0][0], n);
//     qDebug()<<"A[0][0] equals:  "<< A[0][0];


    // Take the complex conjugate of U (Jacobian)
/*
    for(int a = 0; a < n; a++){
        for(int b = 0; b < n; b++){
            U_cnj[a][b] = conj(Temp_J[a][b]);
        }
    }
    */

    // Jacobian_inv = vv * ww * U_cnj
    Multiply_Matrices(&vvww[0][0], &vv[0][0], n, n, &ww[0][0], n);
    Multiply_Matrices(&Jacobian_inv[0][0], &vvww[0][0], n, n, &U_cnj[0][0], n);

// Test****************************************************************************************************
    //int numb = 0;
    for(int e = 0; e < m; e++){
        for(int f = 0; f < n; f++){
            Jacobian_inv[e][f] = 2.2;
            Jacobian[e][f] = 1.25;
            //numb++;
        }
    }

// Test end************************************************************************************************

// Check if the pseudo inverted matrix is ok.
    // If the condition number is greater than the threshold, the robot may be approaching a singularity
    condition_Number = Get_Condition_Number(Jacobian[0][0], Jacobian_inv[0][0], m, n);
//     qDebug()<<"Condition number is:  "<<condition_Number;
    if(condition_Number <= threshold){
        // Solve and update joint parameters
        Multiply_Matrices(&delta_Q[0], &Jacobian_inv[0][0], m, n, &error[0], 1);
        for(int d = 0; d < Last_Link; d++){
            Q[d] += delta_Q[d];
            robot->jointList[d].actuate((float) Q[d]);      // Update the display
        }
//         qDebug()<<"Inverse Kinematics Successful";
        return (1);
    }
    else{
//         qDebug()<<"Inverse Kinematics Failed";
        return (0);
    }
}

void Core::makeJacobian()
{
// This function takes the position of the gripper and returns the 6-by-n Jacobian Matrix

    double Kaxis[3] = {0};       //Ki
    double Center_n[3] = {0};    //On
    double Center_i[3] = {0};    //Oi
    double Difference[3] = {0};  //(On-Oi)
    double Cross[3] = {0};       //Ki X (On-Oi)
    double output[3][4] = {0};

    findCenter(Last_Link, output[0][0]);
    Center_n[0] = output[0][0];
    Center_n[1] = output[1][0];
    Center_n[2] = output[2][0];

    // Loop through each joint and build the Jacobian
    for (int J = 0; J < Last_Link; J++){

        if (type[J] == 'r'){           // Revolute
            findCenter(J, output[0][0]);
            Center_i[0] = output[0][0];
            Center_i[1] = output[1][0];
            Center_i[2] = output[2][0];
            Kaxis[0] = output[0][3];
            Kaxis[1] = output[1][3];
            Kaxis[2] = output[2][3];

            // Get (On-Oi)
            for (int i = 0; i < 3; i++){
                Difference[i] = Center_n[i] - Center_i[i];}

            // Get Ki X (On-Oi)
            Cross_Product(Kaxis[0], Difference[0], Cross[0]);

        // Fill in the items of the Jacobian

            // Fill the top part
            Jacobian[0][J] = Cross[0];
            Jacobian[1][J] = Cross[1];
            Jacobian[2][J] = Cross[2];

            // Fill the bottom part
            Jacobian[3][J] = Kaxis[0];
            Jacobian[4][J] = Kaxis[1];
            Jacobian[5][J] = Kaxis[2];
        }

        else if (type[J] == 'p'){          // Prismatic
            findCenter(J, output[0][0]);
            Kaxis[0] = output[0][3];
            Kaxis[1] = output[1][3];
            Kaxis[2] = output[2][3];

        // Fill in the items of the Jacobian

            // Fill the top part
            Jacobian[0][J] = Kaxis[0];
            Jacobian[1][J] = Kaxis[1];
            Jacobian[2][J] = Kaxis[2];

            // Fill the bottom part
            Jacobian[3][J] = 0;
            Jacobian[4][J] = 0;
            Jacobian[5][J] = 0;
        }
    }
}

void Core::readstr(FILE* f, char* string)
{
        do 
        {
        	fgets(string, 255, f);
        } while ((string[0] == '/') || (string[0] == '#') || (string[0] == 0x0A) || ((string[0] == 0x0D) && (string[1] == 0x0A))); //LF or CR+LF
}
